#ifndef __EM_PLANNER_QP_SOLVER_HPP__
#define __EM_PLANNER_QP_SOLVER_HPP__

#include <iostream>
#include <OsqpEigen/OsqpEigen.h>
#include <Eigen/Dense>
#include <vector>
#include <time.h>

#include "matplotlibcpp.h"
#include "./em_utils.hpp"

 
using namespace std;
using namespace Eigen;
namespace plt = matplotlibcpp;

class QPSolver {
private:
    OsqpEigen::Solver solver;

    Eigen::SparseMatrix<double> hessian;        // Hessian 矩阵
    Eigen::SparseMatrix<double> hessian1;
    Eigen::SparseMatrix<double> hessian2;
    Eigen::VectorXd gradient;                   // 梯度向量
    Eigen::SparseMatrix<double> linearMatrix;   // 线性约束矩阵
    Eigen::VectorXd lowerBound;                 // 下界向量
    Eigen::VectorXd upperBound;                 // 上界向量

    vector<double> ref_x, ref_y;                // 需要平滑的参考线坐标

    bool show;                                  // 是否显示结果

    bool checkMat();

public:
    QPSolver(bool isShow);
    ~QPSolver();
    void initQuinticPoly();
    void updateRefePoint(vector<ReferencePoint> &ref_point);
    void updateRefePoint(vector<TraPoint> &list);
    void updateRefePoint(vector<double> &x, vector<double> &y);
    void initFEMSmoothMat();
    void initFEMSmoothMat(int n, double w_cost_smooth, double w_cost_length, double w_cost_ref, 
                            double lb, double ub);

    void initJerkMat(const vector<double> &l_min, const vector<double> &l_max, 
                            const double host_d1, const double host_d2, const double host_w, 
                            const SLPoint plan_start_sl,
                            const double w_cost_l,
                            const double w_cost_dl, 
                            const double w_cost_ddl, 
                            const double w_cost_dddl,
                            const double w_cost_center, 
                            const double w_cost_end_l,
                            const double w_cost_end_dl, 
                            const double w_cost_end_ddl);
    bool run(vector<Point> &smooth_points);
    bool runQuinticPoly(vector<double> &params);
    bool runJark(vector<double> &path_l, vector<double> &path_dl, vector<double> &path_ddl);
};

#endif